﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using JigLibX.Math;
#endregion

namespace JigLibX.Physics
{
    /// <summary>
    /// Constraints a point on one body to be fixed to a point on another body
    /// </summary>
    public class ConstraintMaxDistance : Constraint
    {
        // maximum relative velocity induced at the constraint points - proportional
        // to the error
        private const float maxVelMag = 20.0f;
        private const float minVelForProcessing = 0.01f;

        // configuration
        private Body body0;
        private Body body1;
        private Vector3 body0Pos;
        private Vector3 body1Pos;
        private float mMaxDistance;
        // stuff that gets updated
        private Vector3 R0;
        private Vector3 R1;
        private Vector3 worldPos;
        private Vector3 currentRelPos0;

        public ConstraintMaxDistance()
        { }

        public ConstraintMaxDistance(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float maxDistance)
        {
            Initialise(body0, body0Pos, body1, body1Pos, maxDistance);

        }

        public void Initialise(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float maxDistance)
        {
            this.body0Pos = body0Pos;
            this.body1Pos = body1Pos;
            this.body0 = body0;
            this.body1 = body1;
            this.mMaxDistance = maxDistance;

            if (body0 != null) this.body0.AddConstraint(this);
            if (body1 != null) this.body1.AddConstraint(this);
        }

        public override void PreApply(float dt)
        {
            //SetUnsatisfied();
            this.Satisfied = false;

            R0 = Vector3.Transform(body0Pos, body0.Orientation);
            R1 = Vector3.Transform(body1Pos, body1.Orientation);

            //MultMatrix33(mR0, mBody0.Orientation, mBody0Pos);
            //MultMatrix33(mR1, mBody1.Orientation, mBody1Pos);

            Vector3 worldPos0 = body0.Position + R0;
            Vector3 worldPos1 = body1.Position + R1;

            worldPos = 0.5f * (worldPos0 + worldPos1);

            // current location of point 0 relative to point 1
            //SubVector3(mCurrentRelPos0, worldPos0, worldPos1);        
            currentRelPos0 = worldPos0 - worldPos1;

        }

        public override bool Apply(float dt)
        {

            this.Satisfied = true;

            bool body0FrozenPre = !body0.IsActive;
            bool body1FrozenPre = !body1.IsActive;

            if (body0FrozenPre && body1FrozenPre)
                return false;

            Vector3 currentVel0 = body0.Velocity + Vector3.Cross(body0.AngVel, R0);
            Vector3 currentVel1 = body1.Velocity + Vector3.Cross(body1.AngVel, R1);

            // predict a new location
            Vector3 predRelPos0 = (currentRelPos0 + (currentVel0 - currentVel1) * dt);

            // if the new position is out of range then clamp it
            Vector3 clampedRelPos0 = predRelPos0;

            float clampedRelPos0Mag = clampedRelPos0.Length();

            if (clampedRelPos0Mag <= JiggleMath.Epsilon)
                return false;

            if (clampedRelPos0Mag > mMaxDistance)
                clampedRelPos0 *= mMaxDistance / clampedRelPos0Mag;

            // now claculate desired vel based on the current pos, new/clamped
            // pos and dt
            Vector3 desiredRelVel0 = ((clampedRelPos0 - currentRelPos0) / System.Math.Max(dt, JiggleMath.Epsilon));

            // Vr is -ve the total velocity change
            Vector3 Vr = (currentVel0 - currentVel1) - desiredRelVel0;

            float normalVel = Vr.Length();

            // limit it
            if (normalVel > maxVelMag)
            {
                Vr *= (maxVelMag / normalVel);
                normalVel = maxVelMag;
            }
            else if (normalVel < minVelForProcessing)
            {
                return false;
            }

            Vector3 N = Vr / normalVel;

            float denominator = body0.InvMass + body1.InvMass +
              Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)) +
              Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R1, N), body1.WorldInvInertia), R1));

            if (denominator < JiggleMath.Epsilon)
                return false;

            float normalImpulse = -normalVel / denominator;

            if (!body0.Immovable)
                body0.ApplyWorldImpulse(normalImpulse * N, worldPos);

            if (!body1.Immovable)
                body1.ApplyWorldImpulse(-normalImpulse * N, worldPos);

            body0.SetConstraintsAndCollisionsUnsatisfied();
            body1.SetConstraintsAndCollisionsUnsatisfied();

            this.Satisfied = true;

            return true;
        }

        public override void Destroy()
        {
            body0 = null;
            body1 = null;

            DisableConstraint();
        }
    }
}
